#pragma once

#include <rclcpp/rclcpp.hpp>
#include <navigation/state_manager.hpp>
#include <navigation/deviation_controller.hpp>
#include <navigation/rviz2_gui/rviz2_connecter.hpp>
#include <navigation/avoid_obstacles/avoid_obstacles.hpp>
#include <navigation/avoid_obstacles/avoid_obstacles.hpp>
#include <std_msgs/msg/string.hpp>
#include <filesystem>

std::string map_manager_dir = MAP_MANAGER_DIR;

class NavCore: public rclcpp::Node {
public:
    NavCore(): Node("nav_core") {
        this->declare_parameter("map_file_path", "");
        this->declare_parameter("targets_file_path", "");
        std::filesystem::path map_file_path = this->get_parameter("map_file_path").as_string();
        targets_file_path = this->get_parameter("targets_file_path").as_string();

        std::string map_file_path_str = map_file_path.string();
        if (map_file_path.empty()) {
            maps_dir_path = map_manager_dir + "/maps";
            if (!std::filesystem::exists(maps_dir_path)) {
                std::filesystem::create_directories(maps_dir_path);
            }
        } else if (map_file_path_str[0] != '/' && map_file_path_str[0] != '~') {
            maps_dir_path = map_manager_dir + "/" + map_file_path.parent_path().string();
        } else {
            maps_dir_path = map_file_path.parent_path().string();
        }

        if (!targets_file_path.empty()) {
            if (targets_file_path[0] != '/' && targets_file_path[0] != '~') {
                targets_file_path = map_manager_dir + "/" + targets_file_path;
            }
        }
        RCLCPP_INFO(this->get_logger(), "Targets file path: %s", targets_file_path.c_str());
    }
    void init() {
        state_manager = std::make_shared<StateManager>(
            shared_from_this(), maps_dir_path, 10.0
        );
        deviation_controller = std::make_shared<EasyDeviationController>(
            shared_from_this(), state_manager, 10.0
        );
        rviz2_connecter = std::make_shared<Rviz2Connecter>(
            shared_from_this(), state_manager, deviation_controller
        );
        avoid_obstacles = std::make_shared<AvoidObstacles>(
            shared_from_this(), deviation_controller
        );

        if (!targets_file_path.empty()) {
            state_manager->load_targets_from_file(targets_file_path);
        }
    }
    std::shared_ptr<StateManager> state_manager;
    std::shared_ptr<DeviationBaseController> deviation_controller;
    std::shared_ptr<Rviz2Connecter> rviz2_connecter;
    std::shared_ptr<AvoidObstacles> avoid_obstacles;
    std::string maps_dir_path;
    std::string targets_file_path;
};
